package sep.test;

import static com.googlecode.javacv.cpp.opencv_core.cvAdd;
import static com.googlecode.javacv.cpp.opencv_core.cvMatMul;
import static com.googlecode.javacv.cpp.opencv_core.cvSetIdentity;

import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D32f;
import com.googlecode.javacv.cpp.opencv_core.CvPoint3D32f;

public class IdaPinholeCamera extends IdaCameraModel {

	private static final long serialVersionUID = 1445509055994810111L;

	private transient CvMat tempVec3;
	private transient CvMat tempVec4;

	public IdaPinholeCamera() {
		create();

		// Standard Parameter
		setCameraParameters(6.047016e+02, 6.043845e+02, -1.810425e-01,
				6.272636e+02, 3.653869e+02);
		setCameraParametersInv(1.653708e-03, 1.654576e-03, 4.953659e-07,
				-1.037492e+00, -6.045604e-01);
		setTranslation(0.0, 1.0, 3.3);
		setRotationAngles(0.0, 265.0, 270.0);
	}

	@Override
	public CvPoint2D32f toCameraCoordinates(final double x, final double y,
			final double z) {

		tempVec4.put(0, 0, x);
		tempVec4.put(1, 0, y);
		tempVec4.put(2, 0, z);
		tempVec4.put(3, 0, 1.0);

		cvMatMul(matP, tempVec4, tempVec4);
		final double zVal = tempVec4.get(2, 0);
		tempVec4.put(0, 0, tempVec4.get(0, 0) / zVal);
		tempVec4.put(1, 0, tempVec4.get(1, 0) / zVal);

		// TODO Test Rückprojektion
		final CvPoint3D32f pr = toStreetCoordinates(tempVec4.get(0, 0),
				tempVec4.get(1, 0));

		return new CvPoint2D32f(tempVec4.get(0, 0), tempVec4.get(1, 0));
	}

	@Override
	public CameraModelType getModelType() {
		return CameraModelType.PinHole;
	}

	@Override
	public CvPoint3D32f toStreetCoordinates(final double x, final double y) {
		tempVec3.put(0, 0, x);
		tempVec3.put(1, 0, y);
		tempVec3.put(2, 0, 1.0);

		// Inverse Intrinsische Parameter anwenden
		synchronized (matCameraInv) {
			cvMatMul(matCameraInv, tempVec3, tempVec3);
		}

		// z rekonstruieren:
		// Es ist bekannt, dass der Punkt eine 2D-Projektion eines 3D-Punktes
		// ist, dessen z-Koordinate = 0 ist. Es muss also so skaliert werden,
		// dass nach der Rücktranslation und Rückrotation des aktuellen
		// 2D-Punktes der so entstandene 3D-Punkt wieder z=0 hat, also:
		//
		// 3D.z = 0 =
		// ir.31(2D.x*s - t.x) + ir.32(2D.y*s - t.y) + r.33(2D.z*s - t.z)
		//
		// wobei ir die inverse Rotationsmatrix und t der Translationsvektor
		// ist. 2D.z ist immer 1. Aufgelöst nach s ergibt sich:
		//
		// ir.31*t.x + ir.32*t.y + ir.33*t.z
		// -------------------------------------- = s
		// ir.31*2D.x + ir.32*2D.y + ir.33*2D.z
		final double s = (translationX * matRotationInv.get(2, 0)
				+ translationY * matRotationInv.get(2, 1) + translationZ
				* matRotationInv.get(2, 2))
				/ (tempVec3.get(0, 0) * matRotationInv.get(2, 0)
						+ tempVec3.get(1, 0) * matRotationInv.get(2, 1) + tempVec3
						.get(2, 0) * matRotationInv.get(2, 2));
		tempVec3.put(0, 0, tempVec3.get(0, 0) * s);
		tempVec3.put(1, 0, tempVec3.get(1, 0) * s);
		tempVec3.put(2, 0, tempVec3.get(2, 0) * s);

		// Punkt verschieben und drehen
		synchronized (vecTranslationInv) {
			cvAdd(tempVec3, vecTranslationInv, tempVec3, null);
		}
		synchronized (matRotationInv) {
			cvMatMul(matRotationInv, tempVec3, tempVec3);
		}

		return new CvPoint3D32f(tempVec3.get(0, 0), tempVec3.get(1, 0),
				tempVec3.get(2, 0));
	}

	@Override
	public void create() {
		super.create();
		tempVec3 = CvMat.create(3, 1);

		cvSetIdentity(matCameraInv, 0.0);
		matCameraInv.put(2, 2, 1.0);

		cvSetIdentity(matK, 0.0);
		cvSetIdentity(matR, 0.0);
		matK.put(2, 2, 1.0);
		matR.put(3, 3, 1.0);
		tempVec4 = CvMat.create(4, 1);
	}

}
